#include "CSerialPort.h"
//#include "cylinder.h"
#include <thread>
#include "getAmmo.h"
#include "CWiFi.h"

using namespace std;
typedef pcl::PointXYZ PointT;

//-----------------------function---------------------
void keyinput();
void enable_raw_mode();
void disable_raw_mode();
void ReadSerial();

//------------------------port & global----------------------
static unsigned char re_msg = 0x02; //���ڽ��ܵ�flag
char mode = 0x02;					//ģʽ�л���0Ϊ��1Ϊ
char c = '1';
float y_min_getammo, y_max_getammo; //-0.04, 0.06
float y_min_cylinder, y_max_cylinder;

int fd;
fd_set fs_read;
struct termios option;
CSerialPort myserialport(fd, option, fs_read); //�򿪴���
#ifdef WIFI
CWiFi myWiFi("192.168.2.163", 45455);
void ReadWiFi();//读取wifi的线程
#endif

//------------------------getAmmo---------------------
pcl::console::TicToc tt;


int main()
{
	std::thread *read_thresd = new std::thread(ReadSerial);
#ifdef WIFI
	myWiFi.start();//连接wifi
	//读取wifi的线程
    std::thread *read_wifi_thread = new std::thread(ReadWiFi);
#endif

	y_min_getammo = -0.065, y_max_getammo = -0.035; //0.035,0.105
	y_min_cylinder = 0, y_max_cylinder = 0;
	thread task1(keyinput);
	while (true)
	{
		cout << char(mode + 0x30) << endl;
		if (mode == 0x02) //getAmmo
		{
			SimpleOpenNIViewer getammo;
			getammo.run();
		}
		else
			usleep(10000);
		// if(mode ==0x03)//
		// {
		// 	cout<<"t";
		// 	FindCylinder cylinder;
		// 	cylinder.cylinder_run();
		// }
	}
}

void ReadSerial()
{
	while (1)
	{
		int send_bytes = 0;
		int n = myserialport.readFromCom_uchar(&re_msg, 1);
		//if(n > 0)
		//cout<<re_msg;
		// if(re_msg == 0x03)
		// {
		// 	continue;
		// }
		switch(re_msg)
		{
			case 0x01:
				mode = 0x01;
				break;
			case 0x02:
				mode = 0x02;
				break;

			default:
				mode = 0x01;
				break;

		}
		usleep(1000);
	}
}

void enable_raw_mode()
{
	termios term;
	tcgetattr(0, &term);
	term.c_lflag &= ~(ICANON | ECHO); // Disable echo as well
	tcsetattr(0, TCSANOW, &term);
}

void disable_raw_mode()
{
	termios term;
	tcgetattr(0, &term);
	term.c_lflag |= ICANON | ECHO;
	tcsetattr(0, TCSANOW, &term);
}

void keyinput()
{
	enable_raw_mode();
	while (1)
	{

		usleep(10);
		c = getchar();
		if (c == '1' || c == '2' || c == '3' || c == '4')
		{
			if (c == '2')
			{
				mode = re_msg = 0x02;
			}
			else if (c == '3')
			{
				mode = re_msg = 0x03;
			}
			else if (c == '4')
			{
				mode = re_msg = 0x04;
			}
			else if (c == '1')
			{
				mode = re_msg = 0x01;
			}
		}
		else if (c == 'z')
		{
			cout << "\ny_min_getammo,y_max_getammo:" << y_min_getammo << "," << y_max_getammo << endl;
			cout << "y_min_cylinder,y_max_cylinder:" << y_min_cylinder << "," << y_max_cylinder << endl;
			getchar();
		}
		else if (c == 'q')
			y_min_getammo += 0.005;
		else if (c == 'a')
			y_min_getammo -= 0.005;
		else if (c == 'w')
			y_max_getammo += 0.005;
		else if (c == 's')
			y_max_getammo -= 0.005;
		else if (c == 'o')
			y_min_cylinder += 0.005;
		else if (c == 'k')
			y_min_cylinder -= 0.005;
		else if (c == 'p')
			y_max_cylinder += 0.005;
		else if (c == 'l')
			y_max_cylinder -= 0.005;
	}
	disable_raw_mode();
	tcflush(0, TCIFLUSH); // Clear stdin to prevent characters appearing on prompt
}

#ifdef WIFI
//读取wifi的线程
void ReadWiFi()
{
    while (1)
    {
        char wifi_read[8] = {0};
        myWiFi.recvData(wifi_read, 8);
		sleep(1);
    }
}
#endif
